#define ANG2RAD 3.14159265358979323846/180.0

#include "d_Math.h"
#include <math.h>
#include <GL/gl.h>
#include <iostream>

using namespace std;
using namespace d_Math;

d_Frustum::d_Frustum()
{
	changed = true;
	first_time = true;
}

void d_Frustum::setCamInternals(float angle, float ratio, float nearD, float farD) 
{

	// store the information
	this->ratio = ratio;
	this->angle = angle;
	this->nearD = nearD;
	this->farD = farD;

	// compute width and height of the near and far plane sections
	tang = (float)tan(ANG2RAD * angle * 0.5) ;
	nh = nearD * tang;
	nw = nh * ratio; 
	fh = farD  * tang;
	fw = fh * ratio;


}

void d_Frustum::draw()
{
	d_Vec3f v000 = nbl;
	d_Vec3f v010 = nbr;
	d_Vec3f v100 = ntl;
	d_Vec3f v110 = ntr;

	d_Vec3f v001 = fbl;
	d_Vec3f v011 = fbr;
	d_Vec3f v101 = ftl;
	d_Vec3f v111 = ftr;

	glDisable(GL_LIGHTING);
	glColor3f(0,1,0);
	glBegin(GL_LINE_LOOP);
		glVertex3fv(v000.p);
		glVertex3fv(v010.p);
		glVertex3fv(v110.p);
		glVertex3fv(v100.p);
		glVertex3fv(v000.p);
		glVertex3fv(v001.p);
		glVertex3fv(v011.p);
		glVertex3fv(v111.p);
		glVertex3fv(v101.p);
		glVertex3fv(v001.p);
	glEnd();
	glBegin(GL_LINES);
		glVertex3fv(v010.p);
		glVertex3fv(v011.p);
		glVertex3fv(v110.p);
		glVertex3fv(v111.p);
		glVertex3fv(v100.p);
		glVertex3fv(v101.p);
	glEnd();
	glEnable(GL_LIGHTING);
}

void d_Frustum::setCamDef(d_Vec3f p, d_Vec3f l, d_Vec3f u) 
{

	d_Vec3f dir,nc,fc,X,Y,Z;

	Z = p - l;
	Z.normalize();

	camp = p;
	camd = Z * (-1);	

	X = cross(u , Z);
	X.normalize();
	
	Y = u;//cross(Z , X);

	nc = p - Z * nearD;
	fc = p - Z * farD;
/*
	pl[NEARP] = new d_Plane(Z *(-1),nc);
	pl[FARP] = new d_Plane(Z,fc);

	d_Vec3f aux,normal;

	aux = (nc + Y*nh) - p;
	aux.normalize();
	normal = cross(aux , X);
	pl[TOP] = new d_Plane(normal,nc+Y*nh);

	aux = (nc - Y*nh) - p;
	aux.normalize();
	normal = cross(X , aux);
	pl[BOTTOM] = new d_Plane(normal,nc-Y*nh);
	
	aux = (nc - X*nw) - p;
	aux.normalize();
	normal = cross(aux , Y);
	pl[LEFT] = new d_Plane(normal,nc-X*nw);

	aux = (nc + X*nw) - p;
	aux.normalize();
	normal = cross(Y , aux);
	pl[RIGHT] = new d_Plane(normal,nc+X*nw);
*/


	ntl = nc + Y * nh - X * nw;
	ntr = nc + Y * nh + X * nw;
	nbl = nc - Y * nh - X * nw;
	nbr = nc - Y * nh + X * nw;

	ftl = fc + Y * fh - X * fw;
	ftr = fc + Y * fh + X * fw;
	fbl = fc - Y * fh - X * fw;
	fbr = fc - Y * fh + X * fw;

	pl[TOP] = new d_Plane(ntr,ntl,ftl);
	pl[BOTTOM] = new d_Plane(nbl,nbr,fbr);
	pl[LEFT] = new d_Plane(ntl,nbl,fbl);
	pl[RIGHT] = new d_Plane(nbr,ntr,fbr);
	pl[NEARP] = new d_Plane(ntl,ntr,nbr);
	pl[FARP] = new d_Plane(ftr,ftl,fbl);
}

int d_Frustum::boxInFrustum(d_AABB *b) 
{
	//return INSIDE;
	if(!changed)
		if(b->last_test_passed)
			return INSIDE;
		else
			return OUTSIDE;
	for(int i=0; i < 6; i++) {

		if (pl[i]->signedDistanceTo(b->getVertexP(pl[i]->normal)) < 0)
		{
			b->last_test_passed = false;
			return OUTSIDE;
		}			
	}
	b->last_test_passed = true;
	return INSIDE;
 }




